#include "Kinematics.h"

void Kinematics::set_wheel_distance(float distance)
{
    wheel_distance = distance;
}

void Kinematics::set_motor_param(uint8_t id,float per_pluse_distance)
{
    motor_param[id].per_pulse_distance = per_pluse_distance;
}

    //运动学正解,将左右轮的速度转换成线速度和角速度
void Kinematics::kinematics_forward(float left_speed,float right_speed,float*out_liner_speed,float*out_anglar_speed)
{
    *out_liner_speed = (right_speed + left_speed)/2;
    *out_anglar_speed = (right_speed - left_speed)/wheel_distance;
}

    //运动学逆解,将线速度和角速度转换成左右轮的速度
void Kinematics::kinematics_inverse(float linear_speed,float angular_speed,float*out_left_speed,float*out_right_speed)
{
    *out_left_speed = linear_speed - angular_speed*wheel_distance/2;
    *out_right_speed = linear_speed + angular_speed*wheel_distance/2;
}

    //更新电机速度和编码器数据
void Kinematics::update_motor_speed(uint64_t current_time,int32_t left_tick,int32_t right_tick)
{

  int64_t delta_ticks[2]={0,0}; //用于存储上一次读取的编码器数值差


        //计算编码器当前和上一次读取数值的差
  int16_t dt = current_time - last_update_time; //ms
    //计算编码器当前和上一次读取数值的差
  delta_ticks[0] = left_tick - motor_param[0].last_encoder_ticks;
  delta_ticks[1] = right_tick - motor_param[1].last_encoder_ticks;

  motor_param[0].motor_speed = delta_ticks[0] * 0.105805/dt*1000;//   mm/s
  motor_param[1].motor_speed = delta_ticks[1] * 0.105805/dt*1000;

  //更新last_ticks
  motor_param[0].last_encoder_ticks = left_tick;
  motor_param[1].last_encoder_ticks = right_tick;
  last_update_time = current_time;

    update_odom(dt);
} 

    //获取电机速度,返回值是速度
int16_t Kinematics::get_motor_speed(uint8_t id)
{
    if(id<0 ||id>1){
        return -1;
    }
    return motor_param[id].motor_speed;
}

odom_t& Kinematics::get_odom()
{
    return odom;
}


void Kinematics::update_odom(int dt)
{
    float dt_s = float(dt)/1000.0; //ms---s
    //获取实时的角速度和线速度
    this->kinematics_forward(motor_param[0].motor_speed,motor_param[1].motor_speed,&odom.linear_speed,&odom.angular_spedd);

    odom.linear_speed = odom.linear_speed/1000.0; //转换成m/s

    //角度积分
    odom.angle +=odom.angular_spedd*dt_s;
    TransAngleInPI(odom.angle,odom.angle);

    //计算机器人行走的距离
    float delta_distance = odom.linear_speed*dt_s;
    //分解到X轴 Y轴
    odom.x += delta_distance*std::cos(odom.angle);
    odom.y += delta_distance*std::sin(odom.angle);

}

void Kinematics::TransAngleInPI(float angle,float&out_angle)
{
    if(angle>PI)
    {
        out_angle -= 2*PI;
    }
    else if(angle<-PI)
    {
        out_angle += 2*PI;
    }
    
}